"""
Lucas-Kanade算法原理
1. 光流假设
Lucas-Kanade算法基于以下假设：在连续的两帧图像中，一个像素点的亮度变化与其运动轨迹之间存在线性关系。

2. 算法基础
算法的核心是一个优化问题，即在给定的一小区域内，寻找一个向量，使得该区域内所有像素点的位移与实际位移的误差平方和最小。

3. 迭代优化
算法采用迭代的方式不断优化位移向量，直到满足一定的精度要求。

实战技巧
1. 特征点选择
选择合适的特征点对于算法的准确性至关重要。通常，特征点应该是亮度变化明显且不易发生变化的区域。

2. 窗口大小
窗口大小的选择会影响算法的鲁棒性和精度。过大可能导致特征点移动过快，过小则可能无法覆盖足够的像素。

3. 多尺度处理
为了处理不同尺度的运动，可以使用多尺度金字塔来提高算法的鲁棒性。

4. 对抗噪声
图像中的噪声可能会影响算法的准确性。使用中值滤波等去噪方法可以提高算法的鲁棒性。
"""
import cv2
import numpy as np

# 初始化视频捕获（引用[[5]]）
cap = cv2.VideoCapture("./modi_tracker/shoulders.mp4")
ret, first_frame = cap.read()
prev_gray = cv2.cvtColor(first_frame, cv2.COLOR_BGR2GRAY)

# 使用Shi-Tomasi算法检测角点（特征点）
feature_params = dict(maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7)
prev_pts = cv2.goodFeaturesToTrack(prev_gray, mask=None, **feature_params)

# 创建光流可视化掩码
mask = np.zeros_like(first_frame)

while(cap.isOpened()):
    ret, frame = cap.read()
    if not ret:
        break
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 计算光流（引用[[4]][[9]]）
    next_pts, status, err = cv2.calcOpticalFlowPyrLK(
        prev_gray, gray, prev_pts, None, 
        winSize=(15,15), maxLevel=2,
        criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
    )

    # 筛选有效光流点
    good_new = next_pts[status==1]
    good_old = prev_pts[status==1]

    # 绘制光流轨迹（引用[[5]]）
    for i,(new,old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()
        c, d = old.ravel()
        mask = cv2.line(mask, (int(a),int(b)), (int(c),int(d)), (0,255,0), 2)
        frame = cv2.circle(frame, (int(a),int(b)), 5, (0,0,255), -1)
    output = cv2.add(frame, mask)

    # 更新前一帧和特征点
    prev_gray = gray.copy()
    prev_pts = good_new.reshape(-1,1,2)

    # 显示结果
    cv2.imshow("Optical Flow", output)
    if cv2.waitKey(30) & 0xFF == 27:  # 按ESC退出
        break

cap.release()
cv2.destroyAllWindows()